#include "robotmotori.h"
#include "tmglobaldef.h"

RobotMotorI::RobotMotorI()
{
    g_pSerialTh = new TmSerialThread();
    g_pSerialTh->start();
}

void RobotMotorI::SerialWrite(const Ice::ByteSeq &bData, const Ice::Current &)
{
    QByteArray bCmd = QByteArray((char*)bData.data(), bData.size());
    g_pSerialTh->msg_push(bCmd);
}

void RobotMotorI::HeadEnable(bool bLR, bool bUD, const Ice::Current &)
{
    g_pSerialTh->HeadEnable(bLR, bUD);
}

void RobotMotorI::HeadLrInit(const Ice::Current &)
{
    g_pSerialTh->HeadLrInit();
}

void RobotMotorI::HeadUdInit(const Ice::Current &)
{
    g_pSerialTh->HeadUdInit();
}

void RobotMotorI::HeadLrAngle(Ice::Int nAngle, Ice::Int nSpeed, const Ice::Current &)
{
    g_pSerialTh->HeadLrAngle(nAngle, nSpeed);
}

void RobotMotorI::HeadUdAngle(Ice::Int nAngle, Ice::Int nSpeed, const Ice::Current &)
{
    g_pSerialTh->HeadUdAngle(nAngle, nSpeed);
}

void RobotMotorI::HandLEnable(bool bSwing, bool bExpand, const Ice::Current &)
{
    g_pSerialTh->HandLEnable(bSwing, bExpand);
}

void RobotMotorI::HandLSwingInit(const Ice::Current &)
{
    g_pSerialTh->HandLSwingInit();
}

void RobotMotorI::HandLExpandInit(const Ice::Current &)
{
    g_pSerialTh->HandLExpandInit();
}

void RobotMotorI::HandLSwingAngle(Ice::Int nAngle, Ice::Int nSpeed, const Ice::Current &)
{
    g_pSerialTh->HandLSwingAngle(nAngle, nSpeed);
}

void RobotMotorI::HandLExpandAngle(Ice::Int nAngle, Ice::Int nSpeed, const Ice::Current &)
{
    g_pSerialTh->HandLExpandAngle(nAngle, nSpeed);
}

void RobotMotorI::HandREnable(bool bSwing, bool bExpand, const Ice::Current &)
{
    g_pSerialTh->HandREnable(bSwing, bExpand);
}

void RobotMotorI::HandRSwingInit(const Ice::Current &)
{
    g_pSerialTh->HandRSwingInit();
}

void RobotMotorI::HandRExpandInit(const Ice::Current &)
{
    g_pSerialTh->HandRExpandInit();
}

void RobotMotorI::HandRSwingAngle(Ice::Int nAngle, Ice::Int nSpeed, const Ice::Current &)
{
    g_pSerialTh->HandRSwingAngle(nAngle, nSpeed);
}

void RobotMotorI::HandRExpandAngle(Ice::Int nAngle, Ice::Int nSpeed, const Ice::Current &)
{
    g_pSerialTh->HandRExpandAngle(nAngle, nSpeed);
}

void RobotMotorI::WheelEnable(bool bEnable, const Ice::Current &)
{
    g_pSerialTh->WheelEnable(bEnable);
}

void RobotMotorI::WheelStop(bool bSlow, const Ice::Current &)
{
    g_pSerialTh->WheelStop(bSlow);
}

void RobotMotorI::WheelRotate(Ice::Int nAngle, Ice::Int nSpeed, const Ice::Current &)
{
    g_pSerialTh->WheelRotate(nAngle, nSpeed);
}

void RobotMotorI::WheelRun(Ice::Int nDistance, Ice::Int nSpeed, const Ice::Current &)
{
    g_pSerialTh->WheelRun(nDistance, nSpeed);
}

void RobotMotorI::WheelRunDiff(bool bLeftUP, Ice::Int nLeftSpeed, bool bRight, Ice::Int nRightSpeed, const Ice::Current &)
{
    g_pSerialTh->WheelRunDiff(bLeftUP, nLeftSpeed, bRight, nRightSpeed);
}

void RobotMotorI::HeadEye(Ice::Int nIndexF, Ice::Int nIndexB, const Ice::Current &)
{
    g_pSerialTh->HeadEye(nIndexF, nIndexB);
}

void RobotMotorI::HeadEye2(Ice::Int nDelay, bool bR, bool bG, bool bB, const Ice::Current &)
{
    g_pSerialTh->HeadEye2(nDelay, bR, bG, bB);
}

void RobotMotorI::HeadEar(Ice::Int nDelay, bool bR, bool bG, bool bB, const Ice::Current &)
{
    g_pSerialTh->HeadEar(nDelay, bR, bG, bB);
}

void RobotMotorI::BodyLed(Ice::Int nDelay, bool bR, bool bG, bool bB, const Ice::Current &)
{
    g_pSerialTh->BodyLed(nDelay, bR, bG, bB);
}

void RobotMotorI::TouchSet(bool bFlash, Ice::Int nColor, Ice::Int nFPS, const Ice::Current &)
{
    g_pSerialTh->TouchSet(bFlash, nColor, nFPS);
}

void RobotMotorI::SensorBatteryValue(Ice::IntSeq &nValList, const Ice::Current &)
{
    int nVal[2] = {0,0};
    g_pSerialTh->SensorBatteryValue(nVal);
    nValList.resize(2);
    nValList[0] = nVal[0];
    nValList[1] = nVal[1];

}

void RobotMotorI::SensorSonicValue(Ice::IntSeq &nValList, const Ice::Current &)
{
    int nVal[MAX_CHAOSHENG] = {0};
    g_pSerialTh->SensorSonicValue(nVal);
    nValList.resize(MAX_CHAOSHENG);
    for(int i=0;i<MAX_CHAOSHENG;i++){
        nValList[i] = nVal[i];
    }

}

void RobotMotorI::SensorInfraredValue(Ice::IntSeq &nValList, const Ice::Current &)
{
    int nVal[MAX_HONGWAI] = {0};
    g_pSerialTh->SensorInfraredValue(nVal);
    nValList.resize(MAX_HONGWAI);
    for(int i=0;i<MAX_HONGWAI;i++){
        nValList[i] = nVal[i];
    }
}

void RobotMotorI::SetProtect(bool bOpen, const Ice::Current &)
{
    g_pSerialTh->SetProtect(bOpen);
}

bool RobotMotorI::GetProtect(const Ice::Current &)
{
    return g_pSerialTh->GetProtect();
}

TmSerialThread::TmSerialThread()
{
    m_pSerialPort = NULL;
    m_pSerialPort_Box = NULL;
    m_bExit = false;
    m_bProtectOpen = true;
}

void TmSerialThread::HeadEnable(bool bLR, bool bUD)
{
    char buff[] = { 0x24, 0x5c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( bUD ){
        buff[3] = 0x01;
    }
    if( bLR ){
        buff[4] = 0x01;
    }
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HeadLrInit()
{
    char buff[] = { 0x24, 0x5d, 0x02, 0x05, 0x00, 0x00, 0x00, 0x00, 0xAA };
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HeadUdInit()
{
    char buff[] = { 0x24, 0x5d, 0x01, 0x05, 0x01, 0x00, 0x00, 0x00, 0xAA };
    if( g_strRobotType.startsWith("TMV3") ){
        buff[4] = 0x00;
    }
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HeadLrAngle(int nAngle, int nSpeed)
{
    char buff[] = { 0x24, 0x5a, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( nSpeed<1||nSpeed>10 ){
        nSpeed = 2;
    }
    buff[3] = (uchar)(nSpeed*10);
    int nReal = 0;
    if( g_strRobotType.startsWith("TMV3") ){
        // TMV3
        nReal = _v3_head_lr[1] - nAngle;
        if( nReal < _v3_head_lr[0] ){
            nReal = _v3_head_lr[0];
        }else if( nReal > _v3_head_lr[2] ){
            nReal = _v3_head_lr[2];
        }
    }else{
        // TMV1
        nReal = _head_lr[1] + nAngle;
        if( nReal < _head_lr[0] ){
            nReal = _head_lr[0];
        }else if( nReal > _head_lr[2] ){
            nReal = _head_lr[2];
        }
    }
    buff[4] = (uchar)nReal;
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HeadUdAngle(int nAngle, int nSpeed)
{
    char buff[] = { 0x24, 0x5a, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( nSpeed<1||nSpeed>10 ){
        nSpeed = 2;
    }
    buff[3] = (uchar)(nSpeed*10);
    int nReal = 0;
    if( g_strRobotType.startsWith("TMV3") ){
        // TMV3
        nReal = _v3_head_ud[1] - nAngle;
        if( nReal < _v3_head_ud[0] ){
            nReal = _v3_head_ud[0];
        }else if( nReal > _v3_head_ud[2] ){
            nReal = _v3_head_ud[2];
        }
    }else{
        // TMV1
        nReal = _head_ud[1] - nAngle;
        if( nReal < _head_ud[0] ){
            nReal = _head_ud[0];
        }else if( nReal > _head_ud[2] ){
            nReal = _head_ud[2];
        }
    }
    buff[4] = (uchar)nReal;
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandLEnable(bool bSwing, bool bExpand)
{
    char buff[] = { 0x22, 0x5c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( bSwing ){
        buff[3] = 0x01;
    }
    if( bExpand ){
        buff[4] = 0x01;
    }
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandLSwingInit()
{
    char buff[] = { 0x22, 0x5d, 0x01, 0x05, 0x00, 0x00, 0x00, 0x00, 0xAA };
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandLExpandInit()
{
    char buff[] = { 0x22, 0x5d, 0x02, 0x05, 0x00, 0x00, 0x00, 0x00, 0xAA };
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandLSwingAngle(int nAngle, int nSpeed)
{
    char buff[] = { 0x22, 0x5a, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( nSpeed<1||nSpeed>10 ){
        nSpeed = 2;
    }
    buff[3] = (uchar)(nSpeed*10);
    int nReal = _hand_left_s[1] + nAngle;
    if( nReal < _hand_left_s[0] ){
        nReal = _hand_left_s[0];
    }else if( nReal > _hand_left_s[2] ){
        nReal = _hand_left_s[2];
    }
    buff[4] = (uchar)nReal;
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandLExpandAngle(int nAngle, int nSpeed)
{
    char buff[] = { 0x22, 0x5a, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( nSpeed<1||nSpeed>10 ){
        nSpeed = 2;
    }
    buff[3] = (uchar)(nSpeed*10);
    int nReal = _hand_left_e[1] - (nAngle - 90);
    if( nReal < _hand_left_e[0] ){
        nReal = _hand_left_e[0];
    }else if( nReal > _hand_left_e[2] ){
        nReal = _hand_left_e[2];
    }
    buff[4] = (uchar)nReal;
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandREnable(bool bSwing, bool bExpand)
{
    char buff[] = { 0x23, 0x5c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( bSwing ){
        buff[3] = 0x01;
    }
    if( bExpand ){
        buff[4] = 0x01;
    }
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandRSwingInit()
{
    char buff[] = { 0x23, 0x5d, 0x01, 0x05, 0x01, 0x00, 0x00, 0x00, 0xAA };
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandRExpandInit()
{
    char buff[] = { 0x23, 0x5d, 0x02, 0x05, 0x00, 0x00, 0x00, 0x00, 0xAA };
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandRSwingAngle(int nAngle, int nSpeed)
{
    char buff[] = { 0x23, 0x5a, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( nSpeed<1||nSpeed>10 ){
        nSpeed = 2;
    }
    buff[3] = (uchar)(nSpeed*10);
    int nReal = _hand_right_s[1] + nAngle;
    if( nReal < _hand_right_s[0] ){
        nReal = _hand_right_s[0];
    }else if( nReal > _hand_right_s[2] ){
        nReal = _hand_right_s[2];
    }
    buff[4] = (uchar)nReal;
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::HandRExpandAngle(int nAngle, int nSpeed)
{
    char buff[] = { 0x23, 0x5a, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( nSpeed<1||nSpeed>10 ){
        nSpeed = 2;
    }
    buff[3] = (uchar)(nSpeed*10);
    int nReal = _hand_right_e[1] - (nAngle - 90);
    if( nReal < _hand_right_e[0] ){
        nReal = _hand_right_e[0];
    }else if( nReal > _hand_right_e[2] ){
        nReal = _hand_right_e[2];
    }
    buff[4] = (uchar)nReal;
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::WheelEnable(bool bEnable)
{
    char buff[] = { 0x22, 0x5C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( bEnable ){
        buff[3] = 0x01;
        buff[4] = 0x01;
    }
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::WheelStop(bool bSlow)
{
    char buff[] = { 0x22, 0x55, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( bSlow ){
        buff[1] = 0x54;
    }
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::WheelRotate(int nAngle, int nSpeed)
{
    g_pSerialTh->m_bIsMoving = true;
    char buff[] = { 0x22, 0x51, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( nAngle > 0 ){
        buff[3] = 0x00;
    }
    // --fix motor bug--
    nAngle *= 4;
    if( nSpeed<1||nSpeed>10 ){
        nSpeed = 2;
    }
    buff[4] = (uchar)(nSpeed*10);
    buff[5] = (uchar)((abs(nAngle)&0xff00)>>8);
    buff[6] = (uchar)(abs(nAngle)&0xff);
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::WheelRun(int nDistance, int nSpeed)
{
    if( nDistance > 0 ){
        g_pSerialTh->m_bIsMoving = true;
    }else{
        g_pSerialTh->m_bIsMoving = false;
    }
    char buff[] = { 0x22, 0x50, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( nDistance > 0 ){
        buff[3] = 0x00;
    }
    if( nSpeed<1||nSpeed>10 ){
        nSpeed = 2;
    }
    buff[4] = (uchar)(nSpeed*10);
    buff[5] = (uchar)((abs(nDistance)&0xff00)>>8);
    buff[6] = (uchar)(abs(nDistance)&0xff);
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::WheelRunDiff(bool bLeftUP, int nLeftSpeed, bool bRight, int nRightSpeed)
{
    //    if( nLeftSpeed<1||nLeftSpeed>10 ){
    //        nLeftSpeed = 2;
    //    }
    //    nLeftSpeed *= 10;

    //    if( nRightSpeed<1||nRightSpeed>10 ){
    //        nRightSpeed = 2;
    //    }
    //    nRightSpeed *= 10;

    char buff[] = { 0x22, 0x53, 0x00, bLeftUP?0:1, (uchar)nLeftSpeed,bRight?1:0, (uchar)nRightSpeed, 0x00, 0xAA };
    msg_push(QByteArray(buff, 9));
}
void _set_eye_param(int nIndex, char* buff){
    switch (nIndex) {
    case 0:
    {
        buff[3] = 0x02;
        buff[4] = 0x01;
        buff[5] = 0x02;
    }
        break;
    case 1:
    {
        buff[3] = 0x03;
        buff[4] = 0x03;
        buff[5] = 0x08;
    }
        break;
    case 2:
    {
        buff[3] = 0x04;
        buff[4] = 0x04;
        buff[5] = 0x08;
    }
        break;
    case 3:
    {
        buff[3] = 0x0b;
        buff[4] = 0x06;
        buff[5] = 0x04;
    }
        break;
    case 4:
    {
        buff[3] = 0x04;
        buff[4] = 0x06;
        buff[5] = 0x04;
    }
        break;
    case 5:
    {
        buff[3] = 0x04;
        buff[4] = 0x01;
        buff[5] = 0x02;
    }
        break;
    case 6:
    {
        buff[3] = 0x08;
        buff[4] = 0x02;
        buff[5] = 0x04;
    }
        break;
    case 7:
    {
        buff[3] = 0x02;
        buff[4] = 0x02;
        buff[5] = 0x02;
    }
        break;
    case 8:
    {
        buff[3] = 0x10;
        buff[4] = 0x04;
        buff[5] = 0x04;
    }
        break;
    case 9:
    {
        buff[3] = 0x0a;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 10:
    {
        buff[3] = 0x10;
        buff[4] = 0x0a;
        buff[5] = 0x04;
    }
        break;
    case 16:
    {
        buff[3] = 0x0a;
        buff[4] = 0x0a;
        buff[5] = 0x02;
    }
        break;
    case 17:
    {
        buff[3] = 0x0a;
        buff[4] = 0x03;
        buff[5] = 0x01;
    }
        break;
    case 18:
    {
        buff[3] = 0x08;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 19:
    {
        buff[3] = 0x0a;
        buff[4] = 0x06;
        buff[5] = 0x02;
    }
        break;
    case 20:
    {
        buff[3] = 0x0a;
        buff[4] = 0x08;
        buff[5] = 0x06;
    }
        break;
    case 21:
    {
        buff[3] = 0x04;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 22:
    {
        buff[3] = 0x10;
        buff[4] = 0x08;
        buff[5] = 0x02;
    }
        break;
    case 23:
    {
        buff[3] = 0x08;
        buff[4] = 0x08;
        buff[5] = 0x02;
    }
        break;
    case 24:
    {
        buff[3] = 0x10;
        buff[4] = 0x06;
        buff[5] = 0x02;
    }
        break;
    case 25:
    {
        buff[3] = 0x10;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 26:
    {
        buff[3] = 0x08;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 27:
    {
        buff[3] = 0x06;
        buff[4] = 0x06;
        buff[5] = 0x01;
    }
        break;
    case 28:
    {
        buff[3] = 0x16;
        buff[4] = 0x0a;
        buff[5] = 0x20;
    }
        break;
    case 29:
    {
        buff[3] = 0x10;
        buff[4] = 0x0a;
        buff[5] = 0x20;
    }
        break;
    case 30:
    {
        buff[3] = 0x08;
        buff[4] = 0x04;
        buff[5] = 0x04;
    }
        break;
    case 31:
    {
        buff[3] = 0x06;
        buff[4] = 0x06;
        buff[5] = 0x04;
    }
        break;
    case 32:
    {
        buff[3] = 0x06;
        buff[4] = 0x06;
        buff[5] = 0x02;
    }
        break;
    case 33:
    {
        buff[3] = 0x08;
        buff[4] = 0x08;
        buff[5] = 0x02;
    }
        break;
    case 34:
    {
        buff[3] = 0x04;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 35:
    {
        buff[3] = 0x08;
        buff[4] = 0x08;
        buff[5] = 0x04;
    }
        break;
    case 36:
    {
        buff[3] = 0x04;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 37:
    {
        buff[3] = 0x08;
        buff[4] = 0x08;
        buff[5] = 0x04;
    }
        break;
    case 38:
    {
        buff[3] = 0x04;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 39:
    {
        buff[3] = 0x08;
        buff[4] = 0x08;
        buff[5] = 0x02;
    }
        break;
    case 40:
    {
        buff[3] = 0x08;
        buff[4] = 0x08;
        buff[5] = 0x02;
    }
        break;
    case 41:
    {
        buff[3] = 0x06;
        buff[4] = 0x06;
        buff[5] = 0x04;
    }
        break;
    case 42:
    {
        buff[3] = 0x10;
        buff[4] = 0x06;
        buff[5] = 0x04;
    }
        break;
    case 43:
    {
        buff[3] = 0x06;
        buff[4] = 0x06;
        buff[5] = 0x04;
    }
        break;
    case 44:
    {
        buff[3] = 0x08;
        buff[4] = 0x08;
        buff[5] = 0x02;
    }
        break;
    case 45:
    {
        buff[3] = 0x10;
        buff[4] = 0x08;
        buff[5] = 0x08;
    }
        break;
    case 46:
    {
        buff[3] = 0x10;
        buff[4] = 0x08;
        buff[5] = 0x10;
    }
        break;
    case 47:
    {
        buff[3] = 0x10;
        buff[4] = 0x0a;
        buff[5] = 0x10;
    }
        break;
    case 48:
    {
        buff[3] = 0x08;
        buff[4] = 0x10;
        buff[5] = 0x08;
    }
        break;
    case 49:
    {
        buff[3] = 0x08;
        buff[4] = 0x06;
        buff[5] = 0x02;
    }
        break;
    case 50:
    {
        buff[3] = 0x04;
        buff[4] = 0x04;
        buff[5] = 0x02;
    }
        break;
    case 100:
    {
        buff[3] = 0x06;
        buff[4] = 0x06;
        buff[5] = 0x02;
    }
        break;
    default:
    {
        if( nIndex < 0 ){
            buff[1] = 0x02; // 静态表情
            buff[2] = 0x00;
        }else{
            buff[2] = 0x64; // 100
            buff[3] = 0x06;
            buff[4] = 0x06;
            buff[5] = 0x02;
        }
    }
        break;
    }
}

void TmSerialThread::HeadEye(int nIndexF, int nIndexB)
{
    char buff[] = { 0x86, 0x01, 0, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    buff[2] = (uchar)nIndexF;
    _set_eye_param(nIndexF, buff);
    msg_push(QByteArray(buff, 9));
    if(g_strRobotType.startsWith("TMV5")){
        buff[0] = 0x87;
        buff[2] = (uchar)nIndexB;
        _set_eye_param(nIndexB, buff);
        msg_push(QByteArray(buff, 9));
    }
}

void TmSerialThread::HeadEye2(int nDelay, bool bR, bool bG, bool bB)
{
    int nDelay_R = 0;
    char buff[] = { 0x02, 0x01, 0x15, (uchar)nDelay_R, (uchar)bR, (uchar)bG, (uchar)bB, 0x00, 0xAA };

    if( nDelay < 0 || nDelay > 10 ){
        nDelay_R = 0;
    }else{
        nDelay_R = nDelay * 10;
    }
    buff[3] = nDelay_R;

    msg_push(QByteArray(buff, 9));

}

void TmSerialThread::HeadEar(int nDelay, bool bR, bool bG, bool bB)
{
    int nDelay_R = 0;
    if( nDelay < 0 || nDelay > 10 ){
        nDelay_R = 0;
    }else{
        nDelay_R = nDelay * 10;
    }
    // right
    char buff[] = { 0x02, 0x01, 0x16, (uchar)nDelay_R, (uchar)bR, (uchar)bG, (uchar)bB, 0x00, 0xAA };
    msg_push(QByteArray(buff, 9));
    if(g_strRobotType.startsWith("TMV5")){
        // left
        buff[2] = 0x15;
        msg_push(QByteArray(buff, 9));
    }
}

void TmSerialThread::BodyLed(int nDelay, bool bR, bool bG, bool bB)
{
    int nDelay_R = 0;
    if( nDelay < 0 || nDelay > 10 ){
        nDelay_R = 0;
    }else{
        nDelay_R = nDelay * 10;
    }
    char buff[] = { 0x02, 0x01, 0x20, (uchar)nDelay_R, (uchar)bR, (uchar)bG, (uchar)bB, 0x00, 0xAA };
    if( g_strRobotType.startsWith("TMV1") ){
        buff[2] = 0x15;
    }
    msg_push(QByteArray(buff, 9));
}

void TmSerialThread::TouchSet(bool bFlash, int nColor, int nFPS)
{
    if(bFlash){
        // right
        char buff[] = { 0x92, 0x01, 0x02, (uchar)(nColor%7), (uchar)nFPS%10+1, 0x00, 0x00, 0x00, 0xAA };
        msg_push(QByteArray(buff, 9));
        // left
        buff[0] = 0x93;
        msg_push(QByteArray(buff, 9));
    }else{
        // right
        char buff[] = { 0x92, 0x01, 0x01, 0/*B*/, 0/*G*/, 0/*R*/, nFPS==0?0:(uchar(255/nFPS)), 0x00, 0xAA };
        switch (nColor) {
        case 0:
        {
            buff[3] = 0x00;
            buff[4] = 0x00;
            buff[5] = 0xff;
        }
            break;
        case 1:
        {
            buff[3] = 0x00;
            buff[4] = 0xff;
            buff[5] = 0x00;
        }
            break;
        case 2:
        {
            buff[3] = 0xff;
            buff[4] = 0x00;
            buff[5] = 0x00;
        }
            break;
        case 3:
        {
            buff[3] = 0x00;
            buff[4] = 0x00;
            buff[5] = 0x00;
        }
            break;
        case 4:
        {
            buff[3] = 0xff;
            buff[4] = 0xff;
            buff[5] = 0xff;
        }
            break;
        case 5:
        {
            buff[3] = 0x00;
            buff[4] = 0xff;
            buff[5] = 0xff;
        }
            break;
        case 6:
        {
            buff[3] = 0xff;
            buff[4] = 0xff;
            buff[5] = 0x00;
        }
            break;
        default:
        {
            buff[3] = 0xff;
            buff[4] = 0xff;
            buff[5] = 0xff;
        }
            break;
        }
        msg_push(QByteArray(buff, 9));
        // left
        buff[0] = 0x93;
        msg_push(QByteArray(buff, 9));
    }
}

void TmSerialThread::SensorBatteryValue(int nVal[])
{
    nVal[0] = pr_nBattery[0];
    nVal[1] = pr_nBattery[1];
}

void TmSerialThread::SensorSonicValue(int nVal[])
{
    for(int i=0;i<MAX_CHAOSHENG;i++){
        nVal[i] = pr_nChaosheng[i];
    }
    // fix bug for plus
    if( nVal[0] == 0 ){
        nVal[0] = nVal[1];
    }
}

void TmSerialThread::SensorInfraredValue(int nVal[])
{
    for(int i=0;i<MAX_HONGWAI;i++){
        nVal[i] = pr_nHongwai[i];
    }
}

void TmSerialThread::SetProtect(bool bOpen)
{
    m_bProtectOpen = bOpen;
}

bool TmSerialThread::GetProtect()
{
    return m_bProtectOpen;
}

void TmSerialThread::run()
{
    qDebug()<<"Load Motor Max Angle...";
    QStringList strParamList = g_pSettingCore->value("Motor/Head_LR", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Motor/Head_LR paramas error!");
        return;
    }
    _head_lr[0] = strParamList.at(0).toInt();
    _head_lr[1] = strParamList.at(1).toInt();
    _head_lr[2] = strParamList.at(2).toInt();

    strParamList = g_pSettingCore->value("Motor/Head_UD", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Motor/Head_UD paramas error!");
        return;
    }
    _head_ud[0] = strParamList.at(0).toInt();
    _head_ud[1] = strParamList.at(1).toInt();
    _head_ud[2] = strParamList.at(2).toInt();

    strParamList = g_pSettingCore->value("Motor/V3_Head_UD", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Motor/V3_Head_UD paramas error!");
        return;
    }
    _v3_head_ud[0] = strParamList.at(0).toInt();
    _v3_head_ud[1] = strParamList.at(1).toInt();
    _v3_head_ud[2] = strParamList.at(2).toInt();

    strParamList = g_pSettingCore->value("Motor/V3_Head_LR", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Motor/V3_Head_LR paramas error!");
        return;
    }
    _v3_head_lr[0] = strParamList.at(0).toInt();
    _v3_head_lr[1] = strParamList.at(1).toInt();
    _v3_head_lr[2] = strParamList.at(2).toInt();

    strParamList = g_pSettingCore->value("Motor/Hand_L_Swing", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Motor/Hand_L_Swing paramas error!");
        return;
    }
    _hand_left_s[0] = strParamList.at(0).toInt();
    _hand_left_s[1] = strParamList.at(1).toInt();
    _hand_left_s[2] = strParamList.at(2).toInt();

    strParamList = g_pSettingCore->value("Motor/Hand_R_Swing", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Motor/Hand_R_Swing paramas error!");
        return;
    }
    _hand_right_s[0] = strParamList.at(0).toInt();
    _hand_right_s[1] = strParamList.at(1).toInt();
    _hand_right_s[2] = strParamList.at(2).toInt();

    strParamList = g_pSettingCore->value("Motor/Hand_L_Expand", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Motor/Hand_L_Expand paramas error!");
        return;
    }
    _hand_left_e[0] = strParamList.at(0).toInt();
    _hand_left_e[1] = strParamList.at(1).toInt();
    _hand_left_e[2] = strParamList.at(2).toInt();

    strParamList = g_pSettingCore->value("Motor/Hand_R_Expand", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Motor/Hand_R_Expand paramas error!");
        return;
    }
    _hand_right_e[0] = strParamList.at(0).toInt();
    _hand_right_e[1] = strParamList.at(1).toInt();
    _hand_right_e[2] = strParamList.at(2).toInt();

    m_bProtectOpen = g_pSettingCore->value("Protect/Open", "").toInt()>0?true:false;
    strParamList = g_pSettingCore->value("Protect/Distance", "").toStringList();
    if( strParamList.size() < 1 ){
        qDebug()<<("Load Protect/Distance paramas error!");
        return;
    }
    _protect_distance[0] = strParamList.at(0).toInt();
    _protect_distance[1] = strParamList.at(1).toInt();

    if( _protect_distance[0]<0 ){
        m_bProtectOpen = false;
        qDebug()<<("Protect/Distance function close!");
    }

    // Serialport
    if( !m_pSerialPort ){
        m_pSerialPort = new QSerialPort();
    }

    for(int i=0; i<MAX_CHAOSHENG; i++){
        pr_nChaosheng[i] = 0;
    }
    for(int i=0; i<MAX_HONGWAI; i++){
        pr_nHongwai[i] = 0;
    }
    for(int i=0; i<MAX_REMIN; i++){
        pr_bRemin[i] = false;
    }
    for(int i=0; i<MAX_TOUCH; i++){
        pr_bTouch[i] = false;
    }
    for(int i=0; i<MAX_HIT; i++){
        pr_bHit[i] = false;
    }

    pr_nBattery[0] = 0;
    pr_nBattery[1] = 0;

    pr_nMotorDisk[0] = 0;
    pr_nMotorDisk[1] = 0;

    pr_bMotorOver[0] = false;
    pr_bMotorOver[1] = false;

    QStringList comParams = g_pSettingCore->value("Motor/COM", "").toStringList();
    if( !m_pSerialPort->isOpen() && comParams.size()>0){
        QString strCom = comParams[0];
        m_pSerialPort->setPortName(strCom);

        if( comParams.size()>1 ){
            int nBauds = comParams[1].toInt();
            if( nBauds == 57600){
                m_pSerialPort->setBaudRate(QSerialPort::Baud57600);
            }else if( nBauds == 115200 ){
                m_pSerialPort->setBaudRate(QSerialPort::Baud115200);
            }else if( nBauds == 9600 ){
                m_pSerialPort->setBaudRate(QSerialPort::Baud9600);
            }
        }else{
            m_pSerialPort->setBaudRate(QSerialPort::Baud57600);
        }
        m_pSerialPort->setDataBits(QSerialPort::Data8);
        m_pSerialPort->setStopBits(QSerialPort::OneStop);
        m_pSerialPort->setParity(QSerialPort::NoParity);
        if( !m_pSerialPort->open(QIODevice::ReadWrite) ){
            qDebug()<<"Serial Port open failed."<<m_pSerialPort->errorString();
            return;
        }
        // serial data comein
        connect(m_pSerialPort, SIGNAL(readyRead()), this, SLOT(slot_new_data()), Qt::DirectConnection);
    }
    qDebug()<<"SerialPort open ok.";

    m_bOpenBoxSP = g_pSettingCore->value("Box/Enable",false).toBool();
    if( m_bOpenBoxSP )
    {
        m_pSerialPort_Box = new QSerialPort();
        QStringList comExtraParams = g_pSettingCore->value("Box/COM", "").toStringList();

        if( !m_pSerialPort_Box->isOpen() && comExtraParams.size()>0){
            QString strCom = comExtraParams[0];
            m_pSerialPort_Box->setPortName(strCom);

            if( comExtraParams.size()>1 ){
                int nBauds = comExtraParams[1].toInt();
                if( nBauds == 57600){
                    m_pSerialPort_Box->setBaudRate(QSerialPort::Baud57600);
                }else if( nBauds == 115200 ){
                    m_pSerialPort_Box->setBaudRate(QSerialPort::Baud115200);
                }else if( nBauds == 9600 ){
                    m_pSerialPort_Box->setBaudRate(QSerialPort::Baud9600);
                }
            }else{
                m_pSerialPort_Box->setBaudRate(QSerialPort::Baud57600);
            }
            m_pSerialPort_Box->setDataBits(QSerialPort::Data8);
            m_pSerialPort_Box->setStopBits(QSerialPort::OneStop);
            m_pSerialPort_Box->setParity(QSerialPort::NoParity);
            if( !m_pSerialPort_Box->open(QIODevice::ReadWrite) ){
                qDebug()<<"Extra Serial Port open failed."<<m_pSerialPort_Box->errorString();
                return;
            }
            // serial data comein
            connect(m_pSerialPort_Box, SIGNAL(readyRead()), this, SLOT(slot_new_data_extra()), Qt::DirectConnection);

        }
    }
    qDebug()<<"Extra SerialPort open ok.";



    m_pTmAutoSend = new QTimer();
    connect(m_pTmAutoSend, SIGNAL(timeout()), this, SLOT(slot_auto_send()), Qt::DirectConnection);
    m_pTmAutoSend->start(333);

    m_bIsMoving = true;

    int nPubIndex = 0;
    QByteArray bCmd;
    while(!m_bExit){
        QCoreApplication::processEvents();
        msleep(20);
        nPubIndex += 1;
        if( nPubIndex > 15 ){
            nPubIndex = 0;
            // pub
            if( g_pRobotNotify ){
                QByteArray bJsonData = make_sensor_json();
                g_pRobotNotify->sendNotify("Robot", std::string(bJsonData.data(), bJsonData.size()));
            }
        }

        // get data and write
        if( msg_pop(bCmd) ){
            //            qDebug()<<"Sensor:"<<bCmd;
            m_pSerialPort->write(bCmd);
        }

        // protect
        static int nProtectIndex = 0;
        nProtectIndex += 1;
        if( nProtectIndex > 8 ){
            nProtectIndex = 0;
            if( m_bIsMoving && m_bProtectOpen){
                for(int i=0; i<MAX_CHAOSHENG;i++){
                    if( pr_nChaosheng[i]>0 && pr_nChaosheng[i]<_protect_distance[0] ){
                        WheelStop(true);
                        m_bIsMoving = false;
                        break;
                    }
                }
                for(int i=0; i<MAX_HONGWAI;i++){
                    if( pr_nHongwai[i]>0 && pr_nHongwai[i]<_protect_distance[1] ){
                        WheelStop(false);
                        m_bIsMoving = false;
                        break;
                    }
                }
            }
        }
    }

    // exit
    m_pSerialPort->close();
}

void TmSerialThread::setExit()
{
    m_bExit = true;
    this->terminate();
}

QByteArray TmSerialThread::make_sensor_json()
{
    QJsonDocument jsDoc;
    QJsonObject jsSensor;
    // 超声波
    QJsonArray jsArrayChaoSheng;
    // fix bug for plus
    if( pr_nChaosheng[0] == 0 ){
        pr_nChaosheng[0] = pr_nChaosheng[1];
    }
    for(int i=0; i<MAX_CHAOSHENG; i++){
        jsArrayChaoSheng.append(pr_nChaosheng[i]);
    }
    jsSensor.insert("chaosheng", jsArrayChaoSheng);
    // 红外测距
    QJsonArray jsArrayHongWai;
    for(int i=0; i<MAX_HONGWAI; i++){
        jsArrayHongWai.append(pr_nHongwai[i]);
    }
    jsSensor.insert("hongwai", jsArrayHongWai);
    // 热敏
    QJsonArray jsArrayRemin;
    for(int i=0; i<MAX_REMIN; i++){
        jsArrayRemin.append(pr_bRemin[i]);
    }
    jsSensor.insert("remin", jsArrayRemin);
    // 碰撞
    QJsonArray hitArrayRemin;
    for(int i=0; i<MAX_HIT; i++){
        hitArrayRemin.append(pr_bHit[i]);
    }
    jsSensor.insert("hit", hitArrayRemin);
    // 触摸
    QJsonArray jsArrayTouch;
    for(int i=0; i<MAX_TOUCH; i++){
        jsArrayTouch.append(pr_bTouch[i]);
    }
    jsSensor.insert("touch", jsArrayTouch);
    // 电池
    QJsonArray jsBattery;
    jsBattery.append(pr_nBattery[0]);
    jsBattery.append(pr_nBattery[1]);
    jsSensor.insert("battery", jsBattery);
    // 码盘数据
    QJsonArray jsMotorDisk;
    jsMotorDisk.append(pr_nMotorDisk[0]);
    jsMotorDisk.append(pr_nMotorDisk[1]);
    jsSensor.insert("motor_disk", jsMotorDisk);
    // 电机过载
    QJsonArray jsMotorOver;
    jsMotorOver.append(pr_bMotorOver[0]);
    jsMotorOver.append(pr_bMotorOver[1]);
    jsSensor.insert("motor_over", jsMotorOver);
    // pack
    QJsonObject jsRoot;
    jsRoot.insert("type", "sensor");
    jsRoot.insert("data", jsSensor);
    jsDoc.setObject(jsRoot);
    return jsDoc.toJson(QJsonDocument::Compact);
}

void TmSerialThread::msg_push(QByteArray bCmd)
{
    IceUtil::Mutex::Lock lock(m_MutexMsg);
    m_ListMsg.push_back(bCmd);
}

bool TmSerialThread::msg_pop(QByteArray &bCmd)
{
    IceUtil::Mutex::Lock lock(m_MutexMsg);
    if(m_ListMsg.isEmpty()){
        return false;
    }
    bCmd = m_ListMsg.front();
    m_ListMsg.pop_front();
    return true;
}

void TmSerialThread::slot_new_data()
{
    m_serialBuff += m_pSerialPort->readAll();
    QByteArray bOneCmd;
    //    qDebug()<<"Buff Size:"<<m_serialBuff.size()<<m_serialBuff[0];
    while( _parseOneCmd(m_serialBuff, bOneCmd) == 0 ){
        if( (uchar)bOneCmd[1] == (uchar)0x02 ){
            //            qDebug()<<"bOneCmd:"<<bOneCmd;
            if( (uchar)bOneCmd[2] == (uchar)0x13 ){
                if(g_strRobotType.startsWith("TMV1") ){

                    if( (bOneCmd[3]&0x01) == 0 ){
                        // head touch
                        pr_bTouch[0] = true;
                    }else{
                        pr_bTouch[0] = false;
                    }

                    if( (bOneCmd[3]&0x02) == 0 ){
                        // left hand touch
                        pr_bTouch[1] = true;
                    }else{
                        pr_bTouch[1] = false;
                    }

                    if( (bOneCmd[3]&0x04) == 0 ){
                        // right hand touch
                        pr_bTouch[2] = true;
                    }else{
                        pr_bTouch[2] = false;
                    }
                }
                if((g_strRobotType.startsWith("TMV1")&& !g_strRobotType.startsWith("TMV1MP")) || g_strRobotType.startsWith("TMV5") ){
                    if( (bOneCmd[3]&0x08) != 0 ){
                        // remin checked
                        pr_bRemin[0] = true;
                    }else{
                        pr_bRemin[0] = false;
                    }
                }
            }
            if( (bOneCmd[2] == (char)0x14) && !g_strRobotType.startsWith("TMV1MP") ){
                // 超声波，单位mm, 范围1m-5m
                pr_nChaosheng[0] = ((uchar)bOneCmd[3])*256 + (uchar)bOneCmd[4];
                if( bOneCmd[3] == (char)0xff ){
                    pr_nChaosheng[0] = 0;
                }
                pr_nChaosheng[1] =  ((uchar)bOneCmd[5])*256 + (uchar)bOneCmd[6];
                if( bOneCmd[5] == (char)0xff ){
                    pr_nChaosheng[1] = 0;
                }
            }
            if( bOneCmd[2] == (char)0x24 ){
                // 超声波，单位mm, 范围1m-5m
                pr_nChaosheng[2] = ((uchar)bOneCmd[3])*256 + (uchar)bOneCmd[4];
                if( bOneCmd[3] == (char)0xff ){
                    pr_nChaosheng[2] = 0;
                }
                pr_nChaosheng[3] =  ((uchar)bOneCmd[5])*256 + (uchar)bOneCmd[6];
                if( bOneCmd[5] == (char)0xff ){
                    pr_nChaosheng[3] = 0;
                }
            }
            if( bOneCmd[2] == (char)0x34 ){
                // 超声波，单位mm, 范围1m-5m
                pr_nChaosheng[4] = ((uchar)bOneCmd[3])*256 + (uchar)bOneCmd[4];
                if( bOneCmd[3] == (char)0xff ){
                    pr_nChaosheng[4] = 0;
                }
                pr_nChaosheng[5] =  ((uchar)bOneCmd[5])*256 + (uchar)bOneCmd[6];
                if( bOneCmd[5] == (char)0xff ){
                    pr_nChaosheng[5] = 0;
                }
            }
            if(g_strRobotType.startsWith("TMV5")){
                if( g_strRobotType.startsWith("TMV5A") ){
                    // V5A 红外传感器不可用，否则返回1d->150造成一直避障开启
                    pr_nHongwai[0] = 0;
                    pr_nHongwai[1] = 0;
                    pr_nHongwai[2] = 0;
                    pr_nHongwai[3] = 0;
                }else{
                    if( bOneCmd[2] == (char)0x17 ){
                        pr_nHongwai[0] = (int)(((uchar)bOneCmd[3]*0.5+0.5)*10);
                        pr_nHongwai[0] = pr_nHongwai[0]>800?0:pr_nHongwai[0];
                        pr_nHongwai[1] = (int)(((uchar)bOneCmd[4]*0.5+0.5)*10);
                        pr_nHongwai[1] = pr_nHongwai[1]>800?0:pr_nHongwai[1];
                        pr_nHongwai[2] = (int)(((uchar)bOneCmd[5]*0.5+0.5)*10);
                        pr_nHongwai[2] = pr_nHongwai[2]>800?0:pr_nHongwai[2];
                        pr_nHongwai[3] = (int)(((uchar)bOneCmd[6]*0.5+0.5)*10);
                        pr_nHongwai[3] = pr_nHongwai[3]>800?0:pr_nHongwai[3];
                    }
                    if( bOneCmd[2] == (char)0x18 ){
                        pr_nHongwai[4] = (int)(((uchar)bOneCmd[3]*0.5+0.5)*10);
                        pr_nHongwai[4] = pr_nHongwai[4]>800?0:pr_nHongwai[4];
                        pr_nHongwai[5] = (int)(((uchar)bOneCmd[4]*0.5+0.5)*10);
                        pr_nHongwai[5] = pr_nHongwai[5]>800?0:pr_nHongwai[5];
                        pr_nHongwai[6] = (int)(((uchar)bOneCmd[5]*0.5+0.5)*10);
                        pr_nHongwai[6] = pr_nHongwai[6]>800?0:pr_nHongwai[6];
                        pr_nHongwai[7] = (int)(((uchar)bOneCmd[6]*0.5+0.5)*10);
                        pr_nHongwai[7] = pr_nHongwai[0]>800?0:pr_nHongwai[7];
                    }
                }
                if( bOneCmd[2] == (char)0x19 ){
                    pr_bHit[0] = bOneCmd[3]==(char)0?true:false;
                    pr_bHit[1] = bOneCmd[4]==(char)0?true:false;
                    pr_bHit[2] = bOneCmd[5]==(char)0?true:false;
                    pr_bHit[3] = bOneCmd[6]==(char)0?true:false;
                }

            }
        }
        // ----------V3 某台机器的特殊协议---------
        if(g_strRobotType.startsWith("TMV3")){
            if( (uchar)bOneCmd[1] == (uchar)0x03 ){
                //           printf("OneCmd: %02X %02X %02X %02X %02X %02X %02X %02X \n", bOneCmd[0]&0xFF,bOneCmd[1]&0xFF,bOneCmd[2]&0xFF,bOneCmd[3]&0xFF,bOneCmd[4]&0xFF,
                //                   bOneCmd[5]&0xFF,bOneCmd[6]&0xFF,bOneCmd[7]&0xFF,bOneCmd[8]&0xFF);
                if( (uchar)bOneCmd[2] == (uchar)0x13 ){
                    if( (bOneCmd[3]&0x01) ){
                        // remin checked
                        pr_bRemin[0] = true;
                    }else{
                        pr_bRemin[0] = false;
                    }
                }
                if( bOneCmd[2] == (char)0x14 ){
                    // 超声波，单位mm, 范围1m-5m
                    pr_nChaosheng[0] = ((uchar)bOneCmd[3])*256 + (uchar)bOneCmd[4];
                    if( bOneCmd[3] == (char)0xff ){
                        pr_nChaosheng[0] = 0;
                    }
                    pr_nChaosheng[1] =  ((uchar)bOneCmd[5])*256 + (uchar)bOneCmd[6];
                    if( bOneCmd[5] == (char)0xff ){
                        pr_nChaosheng[1] = 0;
                    }
                }
            }
        }
        // ---------V3 End---------

        if( (uchar)bOneCmd[1] == (uchar)0x16 ){
            if( bOneCmd[2] == (char)0x02 ){
                pr_nBattery[0] = bOneCmd[3];
                pr_nBattery[1] = bOneCmd[4];
            }
        }
        if( (uchar)bOneCmd[1] == (uchar)0x22 ){
            if( bOneCmd[2] == (char)0x59 ){
                if( bOneCmd[3] == (char)0x00 ){
                    pr_nMotorDisk[0] = (bOneCmd[4]<<8)+(bOneCmd[5]<<4)+bOneCmd[6];
                }
                if( bOneCmd[3] ==(char) 0x01 ){
                    pr_nMotorDisk[1] = (bOneCmd[4]<<8)+(bOneCmd[5]<<4)+bOneCmd[6];
                }
            }
            if( bOneCmd[2] == (char)0x62 ){
                if( bOneCmd[3] == (char)0x00 ){
                    pr_bMotorOver[0] = true;
                }
                if( bOneCmd[3] == (char)0x01 ){
                    pr_bMotorOver[1] = true;
                }
            }
        }
        if( (uchar)bOneCmd[1] == (uchar)0x92 ){
            if((uchar)bOneCmd[2] == (uchar)0x03){
                if( bOneCmd[3]&0x01 ){
                    pr_bTouch[MAX_TOUCH-1] = true;

                }else{
                    pr_bTouch[MAX_TOUCH-1] = false;
                }
            }
        }

    }
}

void TmSerialThread::slot_new_data_extra()
{
    m_extraSerialBuff += m_pSerialPort_Box->readAll();
    QByteArray bOneCmd;
    while( _parseOneCmd(m_extraSerialBuff, bOneCmd) == 0 )
    {
        // ----------V1MP 外置传感器---------
        //        if(g_strRobotType.startsWith("TMV1MP"))
        {
            if( (uchar)bOneCmd[1] == (uchar)0x02 ){
                if( (uchar)bOneCmd[2] == (uchar)0x13 ){
                    if( (bOneCmd[3]&0x01) ){
                        // remin checked
                        pr_bRemin[0] = true;
                    }else{
                        pr_bRemin[0] = false;
                    }
                }
                if( bOneCmd[2] == (char)0x14 ){
                    // 超声波，单位mm, 范围1m-5m
                    pr_nChaosheng[0] = ((uchar)bOneCmd[3])*256 + (uchar)bOneCmd[4];
                    if( bOneCmd[3] == (char)0xff ){
                        pr_nChaosheng[0] = 0;
                    }
                    pr_nChaosheng[1] =  ((uchar)bOneCmd[5])*256 + (uchar)bOneCmd[6];
                    if( bOneCmd[5] == (char)0xff ){
                        pr_nChaosheng[1] = 0;
                    }
                    //PLUS机器人盒子功能修复
                    if(bOneCmd[3] & 0x80  || pr_nChaosheng[0] ==0 ){
                        pr_nChaosheng[0] = pr_nChaosheng[1];
                    }
                }
            }
        }
        // ---------V1MP End---------

    }
}

void TmSerialThread::slot_auto_send()
{
    // 超声波
    char buff[] = { 0x02, 0x01, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
    if( m_bOpenBoxSP ){
        if( m_pSerialPort_Box ){
            buff[0] = 0x02;
            m_pSerialPort_Box->write(buff, 9);
        }
    }else{
        if(g_strRobotType.startsWith("TMV3")){
            buff[0] = 0x03;
        }
        msg_push(QByteArray(buff, 9));
    }

    if( g_strRobotType.startsWith("TMV5") ){
        // 电量检测
        char buff1[] = { 0x16, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
        msg_push(QByteArray(buff1, 9));

        // 红外1/2
        // 距离（cm）=11*0.5
        char buff2[] = { 0x02, 0x01, 0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };
        msg_push(QByteArray(buff2, 9));
        buff2[2] = 0x18;
        msg_push(QByteArray(buff2, 9));
    }
}

int TmSerialThread::_parseOneCmd(QByteArray &inData, QByteArray &outData)
{
    int nDataLen = inData.length();
    if( nDataLen < 9 ){
        return -1; // length too small
    }
    int nLeft = inData.indexOf(0x01,0);
    if( -1 == nLeft ){
        inData.clear();
        return -2; // no data
    }
    if( nDataLen < (nLeft+9) ){
        inData.remove(0, nLeft);
        return -1; // length too small
    }
    if( (char)0xAA == inData[8] ){
        outData = inData.mid(nLeft, 9);
    }
    inData.remove(0, nLeft+9);
    return 0;
}


